import json
import cv2
import numpy as np

view_matrix = np.array([
    [0.028123, 0.994606, 0.099844, 0.000000],
    [-0.983324, 0.045480, -0.176082, 0.000000],
    [-0.179673, -0.093227, 0.979299, 0.000000],
    [0.000000, 0.000000, 0.000000, 1.000000]
])

# 投影矩阵
projection_matrix = np.array([
    [3.914871, 0.000000, 0.000000, 0.000000],
    [0.000000, 1.809004, 0.000000, 0.000000],
    [-0.007647, 0.003489, -1.000001, -1.000000],
    [0.000000, 0.000000, -0.001000, 0.000000]
])

x = (view_matrix @ projection_matrix).T

M = np.load("wcpa/1615021141863_info.npz")
_, tRt = M['verts'], M['R_t']

if __name__ == '__main__':
    image = cv2.imread("data/face.jpg")
    image = cv2.rotate(image, cv2.ROTATE_90_CLOCKWISE)
    img_h, img_w, _ = image.shape
    with open("data/face.json", "r") as f:
        data = json.load(f)

    txt_path = 'resources/projection_matrix.txt'
    M_proj = np.loadtxt(txt_path, dtype=np.float32)

    print(M_proj)

    img_w = 512
    img_h = 512

    M1 = np.array([
        [img_w / 2, 0, 0, 0],
        [0, img_h / 2, 0, 0],
        [0, 0, 1, 0],
        [img_w / 2, img_h / 2, 0, 1]
    ])

    verts3d = np.asarray(data['verDatas'])

    ones = np.ones([verts3d.shape[0], 1])
    verts_homo = np.concatenate([verts3d, ones], axis=1)

    Rt = np.asarray(data['trans']).reshape(4, 4)
    # Rt = np.asarray([[0.9948657, -0.028973944, 0.09696633, 0.0],
    #                  [-0.0113833845, 0.9200231, 0.39169887, 0.0],
    #                  [-0.100560315, -0.39079157, 0.9149696, 0.0],
    #                  [0.024007738, 0.15254067, -0.31370965, 1.0]]).reshape(4, 4)
    print('R: ', Rt)
    print('tR: ', tRt)
    # M_proj = (projection_matrix @ view_matrix)
    verts = verts_homo @ Rt @ M_proj @ M1

    w_ = verts[:, [3]]
    verts = verts / w_

    points2d = verts[:, :2]
    points2d[:, 1] = img_h - points2d[:, 1]
    print(points2d)

    image = np.zeros((img_h, img_w, 3), dtype=np.uint8)
    for p in points2d:
        cv2.circle(image, (int(p[0]), int(p[1])), radius=1, color=(0, 0, 255), thickness=1)
    cv2.imshow("z", image)
    cv2.waitKey(0)

    R = Rt[:3, :3]
    t = Rt[:3, 3]
    print(f"R: {R}")
    print(f"t: {t}")
